#include "xbox.h"
#include <ros/console.h>

XBoxControl::XBoxControl(const std::string filename)
{
    m_fd = open(filename.c_str(), O_RDONLY | O_NONBLOCK);
    if (m_fd < 0)
    {
        ROS_ERROR("failed to open %s", filename.c_str());
    }
}

XBoxControl::~XBoxControl()
{
    if (m_fd >= 0)
        close(m_fd);
}

bool XBoxControl::readData(xbox_map &data)
{
    int len, type, number, value;
    struct js_event js;

    len = read(m_fd, &js, sizeof(js_event));
    if (len < 0)
        return false;

    type = js.type;
    number = js.number;
    value = js.value;

    data.time = js.time;

    if (type == JS_EVENT_BUTTON)
    {
        switch (number)
        {
        case XBOX_BUTTON_A:
            data.a = value;
            break;

        case XBOX_BUTTON_B:
            data.b = value;
            break;

        case XBOX_BUTTON_X:
            data.x = value;
            break;

        case XBOX_BUTTON_Y:
            data.y = value;
            break;

        case XBOX_BUTTON_LB:
            data.lb = value;
            break;

        case XBOX_BUTTON_RB:
            data.rb = value;
            break;

        case XBOX_BUTTON_START:
            data.start = value;
            break;

        case XBOX_BUTTON_BACK:
            data.back = value;
            break;

        case XBOX_BUTTON_HOME:
            data.home = value;
            break;

        case XBOX_BUTTON_LO:
            data.lo = value;
            break;

        case XBOX_BUTTON_RO:
            data.ro = value;
            break;

        default:
            break;
        }
    }
    else if (type == JS_EVENT_AXIS)
    {
        switch (number)
        {
        case XBOX_AXIS_LX:
            data.lx = value;
            break;

        case XBOX_AXIS_LY:
            data.ly = value;
            break;

        case XBOX_AXIS_RX:
            data.rx = value;
            break;

        case XBOX_AXIS_RY:
            data.ry = value;
            break;

        case XBOX_AXIS_LT:
            data.lt = value;
            break;

        case XBOX_AXIS_RT:
            data.rt = value;
            break;

        case XBOX_AXIS_XX:
            data.xx = value;
            break;

        case XBOX_AXIS_YY:
            data.yy = value;
            break;

        default:
            break;
        }
    }
    else
    {
        /* Init do nothing */
    }

    return len > 0;
}